/**
 * @file FSM_State_FrontJump.cpp
 * @author
 * @brief
 * @version 0.1
 * @date 2022-03-20
 *
 * @copyright Copyright (c) 2022
 *
 */
#include "FSM_State_FrontJump.h"

template <typename T>
FSM_State_FrontJump<T>::FSM_State_FrontJump(ControlFSMData<T>* _controlFSMData)
    : FSM_State<T>(_controlFSMData, FSM_StateName::FRONTJUMP, "FRONTJUMP") {
    this->checkSafeOrientation = false;

    this->checkPDesFoot = false;
    this->checkForceFeedForward = false;

    // set Kp Kd

}

template <typename T>
void FSM_State_FrontJump<T>::onEnter() {
    this->nextStateName = this->stateName;

    this->transitionData.zero();

    iter = 0;
 
    _count = 0;

    _b_first_visit = true;

    // initial configuration, position
    Vec3<float> initial_foot_points;    //存放站立时，足端坐标点
    Vec3<float> computeAngle;           //用于计算电机角度的过程变量
    initial_foot_points << 0.0, 0.3, 0;
    computeAuxiliaryAngle(*this->_data->_quadruped, initial_foot_points, &computeAngle);
    //将运算后得到的数据进行处理，存放于initial_jpos中，然后传递给qDes
    initial_jpos(0) = computeAngle(0);
    initial_jpos(1) = computeAngle(1) + computeAngle(2);
    initial_jpos(2) = computeAngle(2) - computeAngle(1);

    start_points << 0.1, 0.20, 0;    //蹲下坐标点
    fin_points << -0.05, 0.34, 0;    //跳跃结束前瞬间坐标点

    computeAngle << 0, 0, 0;    //数据清零
    computeAuxiliaryAngle(*this->_data->_quadruped, start_points, &computeAngle);
    start_jpos(0) = computeAngle(0);
    start_jpos(1) = computeAngle(1) + computeAngle(2);
    start_jpos(2) = computeAngle(2) - computeAngle(1);
    //  set Kp Kd
    printf("Enter FrontJump");
}

template <typename T>
void FSM_State_FrontJump<T>::run() {
    if (_b_running) {
        if (!_Initialization()) {
            ComputeCommand();    // 当 _Initialization() 返回0时，说明计数超过了_waiting_count
        }
    } else {
        _SafeCommand();
    }
    _count++;
    if (_count < _waiting_count)
        _SafeCommand();
    //_curr_time += this->_data->controlParameters->controller_dt;
}

// 初始化
template <typename T>
bool FSM_State_FrontJump<T>::_Initialization() {
    static bool test_initialized(false);
    if (!test_initialized) {
        test_initialized = true;
        printf("Test initialization is done\n");
    }
    if (_count <= _waiting_count) {
        for (int leg = 0; leg < 4; leg++) {
            this->_data->_legController->commands[leg].qDes = initial_jpos;    // 在计数值达到500之前，令狗站好
            for (int jidx = 0; jidx < 3; jidx++) {
                this->_data->_legController->commands[leg].tauFeedForward[jidx] = 0.;    // 力设置为0
                this->_data->_legController->commands[leg].qdDes[jidx] = 0.;             // 速度设置为0
                // this->_data->_legController->commands[leg].kpJoint(jidx, jidx) = 20.; //设置Kp与Kd
                // this->_data->_legController->commands[leg].kdJoint(jidx, jidx) = 2.;
            }
        }
        return true;    // 计数值小于_waiting_count时，返回1
    }
    return false;    // 计数值大于_waiting_count时，返回0
}

//_waiting_count < 计数值 < _star_count时，蹲下
//_star_count < 计数值 < _finish_count时，起跳
//_finish_count < 计数值 < 500时，站立
template <typename T>
void FSM_State_FrontJump<T>::ComputeCommand() {
    if (_waiting_count < _count && _count <= _star_count) {
        for (int leg = 0; leg < 4; leg++) {
            this->_data->_legController->commands[leg].qDes = start_jpos;
        }
    } else if (_star_count < _count && _count <= _finish_count) {
        _SetJPosInterPts(_count, _star_count, _finish_count, start_points, start_points);
    } else if (_finish_count < _count && _count < 20000) {
        for (int leg = 0; leg < 4; leg++) {
            this->_data->_legController->commands[leg].qDes = initial_jpos;
        }
    } else
        _count = 0;
}

// 保持站立姿态
template <typename T>
void FSM_State_FrontJump<T>::_SafeCommand() {
    for (int leg = 0; leg < 4; leg++) {
        this->_data->_legController->commands[leg].qDes = initial_jpos;
        for (int jidx = 0; jidx < 3; jidx++) {
            this->_data->_legController->commands[leg].tauFeedForward[jidx] = 0.;
            this->_data->_legController->commands[leg].qdDes[jidx] = 0.;
        }
    }
}

/**
 * 计算跳的过程
 *
 * @param curr_count 当前的计数值，传入_count
 * @param start_count 开始起跳时的计数值，传入_star_count
 * @param finish_count 跳跃结束后的计数值，传入_finish_count
 * @param ini 起跳前的足端点
 * @param fin 跳跃结束前瞬间的足端点
 */
template <typename T>
void FSM_State_FrontJump<T>::_SetJPosInterPts(const int& curr_count, int start_count, int finish_count,
                                              const Vec3<T>& ini, const Vec3<T>& fin) {
    float a(0.f);
    float b(1.f);
    int whole_count = finish_count - start_count;
    int count = curr_count - start_count;

    if (start_count <= curr_count && curr_count <= finish_count) {
        b = (float)count / (float)whole_count;
        a = 1.f - b;
    }
    Vec3<T> inter_pos = a * ini + b * fin;
    Vec3<float> computeAngle;
    computeAuxiliaryAngle(*this->_data->_quadruped, inter_pos, &computeAngle);
    for (int leg = 0; leg < 4; leg++) {
        this->_data->_legController->commands[leg].qDes[0] = computeAngle(0);
        this->_data->_legController->commands[leg].qDes[1] = computeAngle(1) + computeAngle(2);
        this->_data->_legController->commands[leg].qDes[2] = computeAngle(2) - computeAngle(1);
    }
}

template <typename T>
FSM_StateName FSM_State_FrontJump<T>::checkTransition() {
    this->nextStateName = this->stateName;
    iter++;

    switch ((int)this->_data->controlParameters->control_mode) {
        case K_FRONTJUMP:
            break;

        case K_LOCOMOTION:
            this->nextStateName = FSM_StateName::LOCOMOTION;
            break;

        case K_PASSIVE:
            this->nextStateName = FSM_StateName::PASSIVE;
            break;

        default:
            std::cout << "Bad Request: Cannot transition from" << K_FRONTJUMP << "to"
                      << this->_data->controlParameters->control_mode << std::endl;
            break;
    }
    return this->nextStateName;
}

template <typename T>
TransitionData<T> FSM_State_FrontJump<T>::transition() {
    switch (this->nextStateName) {
        case FSM_StateName::PASSIVE:
            this->transitionData.done = true;
            break;

        case FSM_StateName::LOCOMOTION:
            this->transitionData.done = true;

        default:
            std::cout << "Something went wrong in transition" << std::endl;
            break;
    }
    return this->transitionData;
}

template <typename T>
void FSM_State_FrontJump<T>::onExit() {
    // do nothing
}

template class FSM_State_FrontJump<float>;
